#include "AiAgent.h"

#include "Behaviour.h"
#include "Timer.h"
#include "AiAgentGroup.h"
#include "AIState.h"
#include "FoodSearchState.h"

#define _USE_MATH_DEFINES
#include <math.h>

#include <GL\glut.h>

AIAgent::AIAgent() : m_pAIState(0), m_debugDrawForces(true)
{
	m_maxSpeed = 3.5f;
	m_maxForce = 8;
	m_maxTurnRate = 2;
	m_visionRadius = 6;

	
}


void AIAgent::DecideBehaviour()
{
	//Find neighbours
	m_neighbours = m_pGroup->GetNeighbours(this, 7.5f);

	
	float dt = TheTimer::Instance()->GetDT();

	
		
	Vec3f combinedForces = m_pAIState->Update();

	//Vec3f acc = m_combinedForce * m_invMass;

	Vec3f prevVel = m_vel;
	m_vel += combinedForces * dt;

	//clamp vel
	if(m_vel.LengthSqr() > (m_maxSpeed*m_maxSpeed))
	{
		m_vel.Normalise();
		m_vel *= m_maxSpeed;
	}

	//euler integration
	m_pos += (prevVel + m_vel) * 0.5f * dt;

	//heading and side vec
	if(m_vel.Length() != 0)
	{
		m_heading = m_vel.NormaliseReturn();
	}
	m_heading.Y = 0;

	m_sideVec = m_heading.RotateAboutY(M_PI * 0.5f);
	
	
	m_combinedForce.Zero();
}

void AIAgent::SetAIState(AIState* state)
{
	if(m_pAIState)
	{
		AIState* oldState = m_pAIState;
		m_pAIState = state;
		delete oldState;
	}
	else
	{
		m_pAIState = state;
	}
}

void AIAgent::SetBehaviourAndWeights(BehaviourAndWeightMap& behaviours)
{
	m_behaviourMap = behaviours;
}

AIState* AIAgent::GetAIState()
{
	return m_pAIState;
}

void AIAgent::SetGroup(AIAgentGroup* group)
{
	m_pGroup = group;
}

std::vector<AIAgent*>* AIAgent::GetNeighbours()
{
	return &m_neighbours;
}

float AIAgent::GetMaxSpeed() const
{
	return m_maxSpeed;
}

float AIAgent::GetMaxForce() const
{
	return m_maxForce;
}

float AIAgent::GetVisionRadius() const
{
	return m_visionRadius;
}

